#include "Primitive.h"

namespace common {

Primitive::Primitive(const int & id, const PrimitiveType & type, std::list<Point*> & pointSet):IdAble(id),type(type) {

	std::list<Point*>::iterator it;
	int i=0;
	for(it = pointSet.begin();it!=pointSet.end();it++,i++){
		addPoint(*it);
	}

}
Primitive::Primitive(const int& id, const PrimitiveType& type, Point* p1,Point* p2):IdAble(id),type(type) {
	addPoint(p1); addPoint(p2);
}
bool Primitive::isJacobianDependsOn(Point *i, Point *j) {

	int count = 0;
	pointMap::iterator it;
	for(it= points.begin();it!=points.end();it++){
		if(it->second==i) count ++;
		if(it->second==j) count ++;
	}
	return (count==2);
}
std::list<Point*> Primitive::getAllPoints(void) {

	std::list<Point*> pointSet;
	pointMap::iterator it;
	for(it = points.begin();it!=points.end();it++){
		pointSet.push_back(it->second);
	}
	return pointSet;
}
Point* Primitive::getPoint(const int& id) {
	return points.find(id)->second;
}
void Primitive::addPoint(Point* p){
	this->points.insert(make_pair(points.size(),p));

}
int Primitive::getPointPosition(Point* p){
	pointMap::iterator it;
	for(it = points.begin();it!=points.end();it++){
		if(it->second == p) return it->first;
	}
	return -1;
}
int Primitive::getNumOfPoints() {
	return points.size();
}
void Primitive::relax() {

	pointMap::iterator it;
	for(it = points.begin();it!=points.end();it++){
		it->second->resetAnchor();
	}
}
ublasVector Primitive::getForceInPoint(Point *p) {
	//FIXME - wersja 'ciezka' , wersja lekka zaimplementowa w linii i zakomentowan

	ublasVector F = getForceAll();
	int pos = getPointPosition(p);

	ublasVector vec(2);
	vec(0) = F(pos*2);
	vec(1) = F(pos*2+1);

	return vec;
}
ublasVector Primitive::getForceAll(){ //F = -K *q

	ublasMatrix K = getJacobianAll();
	ublasVector q = ublasVector(getNumOfPoints()*2);
	ublasVector F = ublasVector(getNumOfPoints()*2);

	for(int i=0;i<getNumOfPoints();i++){
		Vector tmp = *getPoint(i) - getPoint(i)->getAnchor();
		q(2*i) 	= tmp.getX();
		q(2*i+1)= tmp.getY();
	}

	F = prod(-K,q);

	return F;
}
ublasMatrix Primitive::getJacobian(Point *i, Point *j) {
	//FIXME byc moze funkcja nie bedzie ta potrzebna
	if(isJacobianDependsOn(i,j)){

		skreal Ks = Config::Ks;
		skreal Kb = Config::Kb;

		int si = getPointPosition(i);
		int sj = getPointPosition(j);

		ublasMatrix K(2,2);

		if(si==sj && si==0){
			//pierwszy
			Vector tmp = Vector(i->getAnchor()-getPoint(1)->getAnchor());
			if(tmp.length()!=0.0f){
				Kb = Config::Kb / tmp.length() ;
			}

			K(0,0) = Ks+Kb ; K(1,1) = Ks+Kb ;

		}else if(si==sj &&  si==getNumOfPoints()-1){
			//ostatni

			Vector tmp = Vector(i->getAnchor()-getPoint(getNumOfPoints()-2)->getAnchor());
			if(tmp.length()!=0.0f){
				Kb = Config::Kb / tmp.length() ;
			}

			K(0,0) = Ks+Kb ; K(1,1) = Ks+Kb ;

		} else if(si==sj){
			// w srodku na przekatnej
			skreal Kb1;
			skreal Kb2;
			Vector tmp1 = Vector(i->getAnchor()-getPoint(si+1)->getAnchor());
			Vector tmp2 = Vector(i->getAnchor()-getPoint(si-1)->getAnchor());
			if(tmp1.length()!=0.0f){
				Kb1 = Config::Kb / tmp1.length() ;
			}
			if(tmp2.length()!=0.0f){
				Kb2 = Config::Kb / tmp2.length() ;
			}

			K(0,0) = 2*Ks+Kb1+Kb2 ; K(1,1) = 2*Ks+Kb1+Kb2 ;

		} else if(abs(si-sj)<2){
			// po bokach =-Kb (i,j)

			Vector tmp = Vector(i->getAnchor()-j->getAnchor());
			if(tmp.length()!=0.0f){
				Kb = Config::Kb / tmp.length() ;
			}
			K(0,0) = -Kb ; 	K(1,1) = -Kb ;
		}

		return K;
	}
	return ublasMatrix(0,0);


}
ublasMatrix Primitive::getJacobianAll(){ // matrix K

	skreal Ks = Config::Ks;
	skreal Kb = Config::Kb;

	;
	ublasMatrix K(ublasMatrixZero(getNumOfPoints()*2,getNumOfPoints()*2));

	for(int i=0;i< (getNumOfPoints()-1);i++){
		Vector tmp = Vector(getPoint(i)->getAnchor()-getPoint(i+1)->getAnchor());
		if(tmp.length()!=0.0f){
			Kb = Config::Kb / tmp.length() ;
		}

		K(2*i+0,2*i+0) += Ks+Kb;	K(2*i+0,2*i+2) = -Kb;
		K(2*i+1,2*i+1) += Ks+Kb; 	K(2*i+1,2*i+3) = -Kb;
		K(2*i+2,2*i+0) = -Kb ;		K(2*i+2,2*i+2) += Ks+Kb;
		K(2*i+3,2*i+1) = -Kb ; 		K(2*i+3,2*i+3) += Ks+Kb;
	}
	return K;

}
std::string Primitive::getTypeToString(){
	return PrimitiveTypeNames[type];
}

std::string Primitive::toString(void){
	std::stringstream os;
	os << "Primitive-"<< PrimitiveTypeNames[type] <<"-"<< getId() <<" -[ " ;
	for(pointMap::iterator it = points.begin();it!=points.end();it++){
		os <<"p" << it->second->getId() << ", ";
	}
	os << "] " ;
	return os.str();
}
Primitive::~Primitive() {
}


}

// namespace common

